#include <ros/ros.h>
#include <std_msgs/String.h>
#include <chrono>
#include <iomanip>
#include <sstream>

std::string getCurrentTimeString()
{
  // 获取当前系统时间
  auto now = std::chrono::system_clock::now();
  // 转换为time_t（秒级时间戳）
  std::time_t now_time = std::chrono::system_clock::to_time_t(now);
  // 转换为本地时间
  std::tm *local_time = std::localtime(&now_time);

  // 格式化时间字符串
  std::ostringstream oss;
  oss << std::put_time(local_time, "%Y-%m-%d %H:%M:%S");

  // 添加毫秒部分
  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                now.time_since_epoch()) %
            1000;
  oss << '.' << std::setfill('0') << std::setw(3) << ms.count();

  return oss.str();
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "my_node");
  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<std_msgs::String>("topic", 10);
  ros::Rate rate(1); // 1Hz

  while (ros::ok())
  {
    std_msgs::String msg;
    std::string currentTime = getCurrentTimeString();
    msg.data = "Hello ROS! Current time: " + currentTime;

    ROS_INFO("Publishing: %s", msg.data.c_str());
    pub.publish(msg);

    rate.sleep();
  }
  return 0;
}